#include "encoder.h"
#include "tim.h"

uint32_t TIM_Period[4] = {4294967295, 65535, 65535, 4294967295};
volatile int16_t Encoder_Overflow_CNT[4] = {0};
volatile int16_t Encoder_Overflow_CNT_Last[4] = {0};
volatile int32_t Encoder_CNT_Increment[4] = {0};
volatile int32_t Encoder_CNT_Last[4] = {0};

int Encoder_Data[4] = {0};

void Encoder_Init()
{
    __HAL_TIM_CLEAR_IT(&HTIM_ENCODER_FL, TIM_SR_UIF);
    HAL_TIM_Base_Start_IT(&HTIM_ENCODER_FL);
    __HAL_TIM_SET_COUNTER(&HTIM_ENCODER_FL, 0);
    __HAL_TIM_CLEAR_IT(&HTIM_ENCODER_FR, TIM_SR_UIF);
    HAL_TIM_Base_Start_IT(&HTIM_ENCODER_FR);
    __HAL_TIM_SET_COUNTER(&HTIM_ENCODER_FR, 0);
    __HAL_TIM_CLEAR_IT(&HTIM_ENCODER_BL, TIM_SR_UIF);
    HAL_TIM_Base_Start_IT(&HTIM_ENCODER_BL);
    __HAL_TIM_SET_COUNTER(&HTIM_ENCODER_BL, 0);
    __HAL_TIM_CLEAR_IT(&HTIM_ENCODER_BR, TIM_SR_UIF);
    HAL_TIM_Base_Start_IT(&HTIM_ENCODER_BR);
    __HAL_TIM_SET_COUNTER(&HTIM_ENCODER_BR, 0);
}

void Encoder_Caculate()
{
    int32_t Tim_CNT[4];
    
    Tim_CNT[0] = __HAL_TIM_GetCounter(&HTIM_ENCODER_FL);
    Tim_CNT[1] = __HAL_TIM_GetCounter(&HTIM_ENCODER_FR);
    Tim_CNT[2] = __HAL_TIM_GetCounter(&HTIM_ENCODER_BL);
    Tim_CNT[3] = __HAL_TIM_GetCounter(&HTIM_ENCODER_BR);

    for(uint8_t i=0; i<4; ++i){
        Encoder_CNT_Increment[i] = (Encoder_Overflow_CNT[i] - Encoder_Overflow_CNT_Last[i])*TIM_Period[i]
                        + Tim_CNT[i] - Encoder_CNT_Last[i];
        Encoder_CNT_Last[i] = Tim_CNT[i];
        Encoder_Overflow_CNT_Last[i] = Encoder_Overflow_CNT[i];    
    }
}   